#include <stdlib.h>
#include <string.h>
#include <stdio.h>
//#include <algorithm>
#include "transmit.h"
#include "capture.h"

#include <pthread.h>
#include <unistd.h>
#include <arpa/inet.h>

#include <sys/time.h>


#define MIN(a,b)	( ((a) < (b)) ? (a) : (b) )
#define NUM_CAMERAS		2
//#define VIDEO_WIDTH		640
//#define VIDEO_HEIGHT		480
//#define PACKETS_PER_FRAME	20
#define PACKETS_PER_FRAME	15
/*#define VIDEO_WIDTH		320
#define VIDEO_HEIGHT		240
#define PACKETS_PER_FRAME	20*/


const int port = 2525;
//const int max_packet_size = VIDEO_WIDTH * VIDEO_HEIGHT * 3 / PACKETS_PER_FRAME;
const int max_packet_size = 640 * 480 * 3 / PACKETS_PER_FRAME;
const int fps = 30;
const int pauseMicros = 500 * 1000;


//static struct timeval startTv;
//static struct timeval endTv;
static int * socks = NULL;


void reset_camera(const char * msg, int camera) {
    fprintf(stderr, "%s (camera: %d)\n", msg, camera);
    fprintf(stderr, "Trying to reopen camera %d...\n", camera);
    int err = 1;

    while (err) {
        err = init_cameras(camera);
        usleep(pauseMicros);
    }

    fprintf(stderr, "Camera %d successfully reopened\n", camera);
}


void sendFrame(unsigned int cam, int sock, char * b, struct sockaddr_in * dstAddr) {
    int meta[3];
    char * buffer = NULL;

struct timeval startTv;
struct timeval endTv;

    if (!sock) return;

gettimeofday(&startTv, NULL);

        int size = get_frame(cam, &meta[1], &meta[2], &buffer);

/*gettimeofday(&endTv, NULL);
double start = startTv.tv_sec*1000.0 + startTv.tv_usec/1000.0;
double end = endTv.tv_sec*1000.0 + endTv.tv_usec/1000.0;*/

if (cam == 0) {
//  printf("getframe time: %lf ms (%lf %lf) (s %ld %ld) (e %ld %ld)\n", end-start, start, end, startTv.tv_sec, startTv.tv_usec, endTv.tv_sec, endTv.tv_usec);
}

        if (size <= 0) {
            deinit_camera(cam);
            reset_camera("Error reading frame from camera", cam);
        }

    dstAddr->sin_port = htons(port + cam);
    meta[0] = cam;

//meta[1] = htonl(meta[1]);
//meta[2] = htonl(meta[2]);

/*printf("sending res %ld -- %d, %d\n", sizeof(int)*2, meta[1], meta[2]);

        if (sendto(sock, meta, sizeof(int)*4, 0, (struct sockaddr *) dstAddr, sizeof(struct sockaddr_in)) < 0) {
            fprintf(stderr, "Error sending data\n");
            perror("sendto");
            exit(-1);
        }*/

        int curr = 0;

//gettimeofday(&startTv, NULL);

        while (curr < size) {
            int amt = MIN(size-curr, max_packet_size);

//printf("sending %d\n", amt);

((int *) b)[0] = meta[0];
((int *) b)[1] = meta[1];
((int *) b)[2] = meta[2];
((int *) b)[3] = curr;
memcpy(&b[sizeof(int)*4], &buffer[curr], amt);

//fprintf(stderr, "sending %d, %u, %u, %u\n", ((int *) b)[0], ((int *) b)[1], ((int *) b)[2], ((int *) b)[3]);
//printf("sending offset %d\n", curr);

//gettimeofday(&startTv, NULL);

            if (sendto(sock, b/*&buffer[curr]*/, amt+sizeof(int)*4, 0, (struct sockaddr *) dstAddr, sizeof(struct sockaddr_in)) < 0) {
                fprintf(stderr, "Error sending data\n");
                perror("sendto");
                exit(-1);
            }
/*
gettimeofday(&endTv, NULL);
double start = startTv.tv_sec*1000.0 + startTv.tv_usec/1000.0;
double end = endTv.tv_sec*1000.0 + endTv.tv_usec/1000.0;
//printf("sendto time: %lf ms (%lf %lf) (s %ld %ld) (e %ld %ld)\n", end-start, start, end, startTv.tv_sec, startTv.tv_usec, endTv.tv_sec, endTv.tv_usec);
printf("sendto time: %lf ms\n", end-start);
*/
            curr += amt;

//sleep(1);
            //usleep(1000);
        }

gettimeofday(&endTv, NULL);
double start = startTv.tv_sec*1000.0 + startTv.tv_usec/1000.0;
double end = endTv.tv_sec*1000.0 + endTv.tv_usec/1000.0;

printf("%d fps: %f\n", cam, 1000.0f/(end-start));

if (cam == 0) {
//  printf("transmit time: %lf ms (%lf %lf) (s %ld %ld) (e %ld %ld)\n", end-start, start, end, startTv.tv_sec, startTv.tv_usec, endTv.tv_sec, endTv.tv_usec);
}
}


void * threadEntry(void * data) {
    int i = (int) data;
    struct sockaddr_in dstAddr;

fprintf(stderr, "starting helper thread: %d\n", i);

    memset(&dstAddr, 0, sizeof(dstAddr));
    dstAddr.sin_family = AF_INET;
//    dstAddr.sin_port = htons(port);
//    dstAddr.sin_addr.s_addr = inet_addr("192.168.3.255"); //inet_addr("127.0.0.1"); //INADDR_BROADCAST;
    dstAddr.sin_addr.s_addr = inet_addr("255.255.255.255");

int iter = 0;
char * b = NULL;
b = (char *) malloc(max_packet_size + sizeof(int)*4);

    while (1) {
//        printf("%u) loop iter %d\n", i, iter);

    if (iter % (fps*5) == 0) {
// gettimeofday(&startTv, NULL);

    deinit_camera(i);

    int err = init_cameras(i);

    if (err)
        reset_camera("Error during camera auto-reset", i);
/*
 gettimeofday(&endTv, NULL);
 double start = startTv.tv_sec*1000.0 + startTv.tv_usec/1000.0;
 double end = endTv.tv_sec*1000.0 + endTv.tv_usec/1000.0;
 //printf("sendto time: %lf ms (%lf %lf) (s %ld %ld) (e %ld %ld)\n", end-start, start, end, startTv.tv_sec, startTv.tv_usec, endTv.tv_sec, endTv.tv_usec);
 printf("init_camera time: %lf ms\n", end-start);
*/
    }

//gettimeofday(&startTv, NULL);

/*        if ((iter % 20) > 10) {
            sendFrame(0, &socks[0], b, &dstAddr);
fprintf(stderr, "cam 0\n");
        } else {
            sendFrame(1, socks[1], b, &dstAddr);
fprintf(stderr, "cam 1\n");
        }*/

//        for (int i = 0; i < NUM_CAMERAS; i++)
            sendFrame(i, socks[i], b, &dstAddr);

        iter++;

        //usleep(1000);
//break;

//if (iter > 10)
//  sleep(1);
    }
}


int main() {
    socks = create_datagram_sockets(NUM_CAMERAS);

    if (!socks) {
        fprintf(stderr, "Could not allocate memory for sockets\n");
        exit(-1);
    }

    pthread_t helpers[NUM_CAMERAS-1];

    for (int i = 1; i < NUM_CAMERAS; i++) {
        pthread_create(&helpers[i-1], NULL, threadEntry, (void *) i);
    }

    threadEntry((void *) 0);

    printf("test successful\n");

    return 0;
}
